/*! \mainpage
 * This code contains two programs to deal with the VisGraB database:
 * - prepareExperiments.cpp To access the images, disparity maps, and point clouds of all scene in 
 *   the database, to be used by the user's own grasp-generation method, and to write an
 *   experiment XML file, which is used by the grasp simulator to evaluate the grasps. The
 *   code furthermore creates collision-info files for the scenes. This code can be replaced
 *   by the user's own scene-representation code.
 * - readResults.cpp To read the result XML file provided by the grasp simulator and to 
 *   write data files to show the result using the matlab script visgrabAnalyzeResults.m.
 * \author Gert Kootstra, email: kootstra@kth.se
 * \date 2011
 */

#include "AllGrasps.h"
#include "visgrabTypes.h"
#include "Database.h"
#include "MyGraspGenerator.h"


enum VisgrabInputType { INPUT_VISGRAB, INPUT_VISGRAB_FULL };
 
/*! 
 * \file prepareExperiments.cpp
 * The program prepareExperiments shows how to access all the scenes in the dataset, 
 * to call the user's own grasp-generation 
 * method, and to write all grasp information to an experiment XML. This XML file can
 * then be loaded by the grasp simulator to evaluate the grasps. The grasp simulator 
 * returns a result XML file that can be parsed by the executable 'readResults' and
 * displayed by 'makeGraspResultsTables.m'.
 *
 * The class Database is used to access all information about the scenes in the benchmark.
 * The information about the database can be loaded in two ways: 1) Using visgrab.xml. This
 * is an abstract XML file describing all conditions and scenes available in the 
 * VisGraB benchmark and information about the location of the dataset and the simulator 
 * files. 2) Using visgrabFull.xml. This is a XML file that contains information about
 * each scene in an individual field. For most users of the benchmark, we advice to use (1).
 * If the user wants to experiment with only part of the database, lines can be commented out
 * in the XML files or information can be altered.
 *
 * The user only needs to change a few line in the main function (marked with USER_NOTE) and add
 * his/her own grasp-generation method to the class MyGraspGenerator. Comments in the main 
 * function and the MyGraspGenerator class will help the user to adapt the code. 
 *
 * The grasps generated by the user's method are combined in the class AllGrasps. This class is also
 * used to write the experiment XML file.
 */
int main(int argc, char *argv[]) {

  if(argc < 3) {
    std::cout << "Usage: prepareExperiments <VisgrabManifest.xml> <VisgrabExperiment.xml> [<PreShapes.xml>]" << std::endl;
    exit(EXIT_FAILURE);
  }

  
  string visgrabManifest = string(argv[1]);
  string visgrabExperiment = string(argv[2]);
  string preShapesFile = "";
  if(argc==4)
    preShapesFile = string(argv[3]);

  using boost::filesystem::path;
  path manifestPath(visgrabManifest);
  path experimentPath(visgrabExperiment);

  if(!exists(manifestPath)) {
    std::cout << "Manifest file does not exist" << std::endl;
    exit(EXIT_FAILURE);
  }

  if(extension(manifestPath)!=".xml") {
    std::cout << "Manifest file should be an XML file" << std::endl;
    exit(EXIT_FAILURE);
  }
  if(extension(experimentPath)!=".xml") {
    std::cout << "Experiment file should be an XML file" << std::endl;
    exit(EXIT_FAILURE);
  }

  if(argc==4) {
    path preShapesPath(preShapesFile);
    if(!exists(preShapesPath)) {
      std::cout << "PreShapes file does not exist" << std::endl;
      exit(EXIT_FAILURE);
    }
    if(extension(preShapesPath)!=".xml") {
      std::cout << "PreShapes file should be an XML file" << std::endl;
      exit(EXIT_FAILURE);
    }
  }

  string rootPath = manifestPath.branch_path().string();
  string experimentRootPath = experimentPath.branch_path().string();

  /*!
   * Step 1. Get information from all the scenes.
   */
  Database database;
  std::vector<SceneInfo*> scenesInfo;

  database.readDatabaseInfo(scenesInfo, visgrabManifest);

  /*!
   * USER_NOTE: set the directory where all the collision-info files will be stored.
   * This is relative to the directory where the manifest file is located (rootPath)
   */
  database.addCollisionInfoDirectory(scenesInfo, "experiment/collisionInfo");

  /*!
   * Step 2. Go through all scenes, get the visual input, and generate grasps
   */
  MyGraspGenerator myGraspGenerator;

  /*! 
   * A vector containing all the graps for all the scenes. 
   * One of three grasp representations can be used here: 
   *   - GraspRepresentationI   Based on contact point information.
   *   - GraspRepresentationII  Based on hand pose and grasp pre-shapes.
   *   - GraspRepresentationIII Based on hand pose and joint configuration.
   * USER_NOTE: Uncomment the preferred representation.
   */
  AllGrasps<GraspRepresentationI> *allGrasps = new AllGrasps<GraspRepresentationI>(preShapesFile);
  //AllGrasps<GraspRepresentationII> *allGrasps = new AllGrasps<GraspRepresentationII>(preShapesFile);
  //AllGrasps<GraspRepresentationIII> *allGrasps = new AllGrasps<GraspRepresentationIII>(preShapesFile);

  /*! Loop over all scene to gather all grasps for all scenes. */
  for(unsigned int i=0; i<scenesInfo.size(); i++) {
    std::cout << "Processing scene: " << scenesInfo[i]->sceneLocator << std::endl;

    /*! Get the images, disparity map, and point cloud for this scene. */
    SceneData* sceneData = database.getSceneData(scenesInfo[i], rootPath);

    /*!
     * Get the grasps for this scene. 
     * USER_NOTE: Call your own grasp generation method here. 
     * NB. Use one of the three representations by uncommenting it.
     */
    std::vector<GraspRepresentationI*> sceneGrasps;
    //std::vector<GraspRepresentationII*> sceneGrasps;
    //std::vector<GraspRepresentationIII*> sceneGrasps;
    myGraspGenerator.getGrasps(sceneData, sceneGrasps);

    /*!
     * Get the collsion information.
     * USER_NOTE: This adds the collision information. A default method is implemented that
     * detects the dominant horizontal plane in the point cloud and store that and all the
     * points above the plane in the collision-info file. This file is used by the grasp
     * simulation to decide if grasps can be performed without penetrating elements in
     * the scene.
     */
    myGraspGenerator.makeCollisionInfoFile(sceneData, scenesInfo[i], experimentRootPath);

    /*! Add the grasps to allGrasps */
    allGrasps->addSceneGrasps(sceneGrasps, scenesInfo[i]);

    // Free memory
    delete sceneData;
  }

  /*!
   * Step 3. Write the experiment XML file
   */
  allGrasps->writeExperimentXML(visgrabExperiment);

  // Free memory
  delete allGrasps; //!< This deletes all grasps in all scenes and all sceneInfo


}
